#include "CurveAgent.h"

using namespace TrackInfo;

const float CurveAgent::MAX_DISTANCE = 1/200.0f;
const float CurveAgent::MAX_SPEED = 1 / 300.0f;

#include <cmath>

using namespace MathUtility;

CurveAgent::CurveAgent()
: PotentialField(), aligning(false), last_state(TURN_NONE)
{
	for (int i = 0; i < 5; i++)
	{
		angles[i] = -90 + i*15;
		angles[18-i] = 90 - i*15;
	}
	
	for (int i = 5; i < 9; i++)
	{
		angles[i] = -20+(i-5)*5;
		angles[18-i] = 20-(i-5)*5;
	}
	//angles[8] = -0.5f;
	//angles[10] = 0.5f;
	angles[9] = 0;
	
	//angles[1] = -0.05f;
	
	//angles[17] = 0.05f;

	
	
	last = 0;
	for (int i = 0; i < 10; i++)
	{
		turn[i] = 0;
	}
	marker = 0;
	
	turn_nr = 0;

	last_turn = -1;
}

CurveAgent::~CurveAgent()
{

}

void CurveAgent::update(CarState& cs, const float dt)
{
}

void CurveAgent::update(CarState& cs, const float dt, TrackLearner& track, const float speed)
{
	current = cs;




	this->speed = speed;
	TrackNode node;
	TrackNode current = track.getClosestNode(cs.getDistFromStart());
	clear();
	
	float distance = track.getDistToNextTurn(cs.getDistFromStart(), &node);
	
	int p = 0;
	for (int i = 1; i < 19; i++)
	{
		if (cs.getTrack(i) > cs.getTrack(p))
		{
			p = i;
		}
	}
	
	//std::cout << distance << std::endl;
	




	if (  cs.getFocus(0) > -1)
	{
		if (cs.getFocus(0) > cs.getFocus(4))
		{
			last_turn = 1;
		}
		else if (cs.getFocus(4) > cs.getFocus(0))
		{
			last_turn = 2;
		}
	}


	if (last_turn == 1)
	{
		Charge charge;

		Vector2 pos(speed*dt, right);
		charge.setAngle(pos.angle());
		charge.setLength(pos.length());
		charge.setCharge(5 * calculateK(cs.getTrack(9)));
		charge.update();
		//std::cout << "align to right" << std::endl;
		insertCharge(charge, 0);


	}
	else if (last_turn == 2)
	{
		Charge charge;

		Vector2 pos(speed*dt, -1*left);
		charge.setAngle(pos.angle());
		charge.setLength(pos.length());
		charge.setCharge(5 * calculateK(cs.getTrack(9)));
		charge.update();
		//std::cout << "align to left" << std::endl;
		insertCharge(charge, 0);

	}
	
}

const bool CurveAgent::getAligning() const
{
	return aligning;
}

void CurveAgent::calculatePotentialForOptions(DriverOption* const options, unsigned int size)
{
	if (field.size() > 0)
	{
		for (unsigned int i = 0; i < size; i++)
		{
			float length = 0.0f;
			
			
			
			length = (field[0].getPosition() - options[i].getPosition()).length();
			
			float width = (left + right);
			float charge = field[0].getCharge();
			
			float potential = (1-(length/width));
			
			if (potential < 0.0f)
				potential = 0.0f;
			
			options[i].addPotential(potential * charge);

		}
	}
}

void CurveAgent::setLeftAndRight(const float left, const float right)
{
	this->left = left;
	this->right = right;
}

const float CurveAgent::calculateK(const float lengthToCurve)
{
	return (-1*powf(150-lengthToCurve,2.0f)*0.001) + 10;
}
